#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sensor_msgs/CompressedImage.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <stdio.h>
#define _USE_MATH_DEFINES
#include <boost/thread/thread.hpp>
#include <cmath>
#include <boost/format.hpp>
#include <boost/filesystem.hpp>
#include <string.h> 	 	
#include "viso.h"
#include <iostream>

using cv::Mat;
using boost::format;
using Eigen::MatrixXf;
using Eigen::MatrixXd;
using Eigen::Affine3f;

namespace fs = boost::filesystem;
using namespace cv;
using namespace std;
using cv::DataType;

fs::path result_dir;
boost::mutex inputData;
vector<sensor_msgs::CompressedImage>* my_left;
vector<sensor_msgs::CompressedImage>* my_right;
odometry odometry_object;

bool loadCalib(const string& file_name, Mat& p1, Mat& p2)
{
  FILE *fp = fopen(file_name.c_str(),"r");

  if (!fp)
      return false;
  int n,t;
  t = fscanf(fp, "P%d:", &n);
  assert(t==1);
  t=fscanf(fp, "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf\n",
           &p1.at<double>(0,0), &p1.at<double>(0,1), &p1.at<double>(0,2), &p1.at<double>(0,3),
           &p1.at<double>(1,0), &p1.at<double>(1,1), &p1.at<double>(1,2), &p1.at<double>(1,3),
           &p1.at<double>(2,0), &p1.at<double>(2,1), &p1.at<double>(2,2), &p1.at<double>(2,3));
  assert(t==12);
  t=fscanf(fp, "P%d:", &n);
  assert(t==1);
  t=fscanf(fp, "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
           &p2.at<double>(0,0), &p2.at<double>(0,1), &p2.at<double>(0,2), &p2.at<double>(0,3),
           &p2.at<double>(1,0), &p2.at<double>(1,1), &p2.at<double>(1,2), &p2.at<double>(1,3),
           &p2.at<double>(2,0), &p2.at<double>(2,1), &p2.at<double>(2,2), &p2.at<double>(2,3));
  assert(t==12);
  fclose(fp);
  return true;
}

bool savePoses(const string& file_name, const vector<Mat>& poses) {
    FILE *fp = fopen(file_name.c_str(),"w+");
    if (!fp)
        false;
    for (auto &pose: poses)
    {
        int t=fprintf(fp, "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf\n",
                      pose.at<double>(0,0), pose.at<double>(0,1), pose.at<double>(0,2), pose.at<double>(0,3),
                      pose.at<double>(1,0), pose.at<double>(1,1), pose.at<double>(1,2), pose.at<double>(1,3),
                      pose.at<double>(2,0), pose.at<double>(2,1), pose.at<double>(2,2), pose.at<double>(2,3));
        assert(t>0);
    }
    fclose(fp);
    return true;
}

void callOdometry()
{
  inputData.lock();
    if(!my_left->empty() && !my_right->empty())
    {
     /* 
      Mat left_3x3;
      Mat right_3x3;
      Mat left_3x4;
      Mat right_3x4;
      Mat out_4x4;
      double coordinates[] = {0,0,0.2};
      std::vector<double> cor_vec(coordinates, coordinates + sizeof(coordinates) / sizeof(int) ); //represent the vector between coordinate systems of the cameras.
      
      imshow("before rect" , Mat((my_left->at(0)).data));
      printf("!!!");
      //rectify
      stereoRectify(Mat((my_left->at(0)).data), 		//InputArray cameraMatrix1
			  0,					//InputArray distCoeffs1
			  Mat((my_right->at(0)).data), 	//InputArray cameraMatrix2
			  0,  					//InputArray distCoeffs1
			  Mat((my_right->at(0)).data).size() , //Size imageSize
		          1, 					//Rotation matrix between the coordinate systems of the first and the second cameras
			  cor_vec,				//Translation vector between coordinate systems of the cameras.
			  left_3x3, 				//Output 3x3 rectification transform (rotation matrix) for the first camera
			  right_3x3,				//Output 3x3 rectification transform (rotation matrix) for the second camera
			  left_3x4, 				//Output 3x4 projection matrix in the new (rectified) coordinate systems for the first camera.
			  right_3x4, 				//Output 3x4 projection matrix in the new (rectified) coordinate systems for the second camera.
			  out_4x4,				//Output ￼ disparity-to-depth mapping matrix
			  CALIB_ZERO_DISPARITY,
			  -1, 
			  Size(0,0), 
			  0,
			  0 );
      */
      
      
      Mat m1 = imdecode(Mat((my_left->at(0)).data),0);   
      Mat m2 = imdecode(Mat((my_right->at(0)).data),0); 
      my_left ->erase (my_left ->begin()+0);
      my_right->erase (my_right->begin()+0);

      waitKey(1);
      StereoImageGenerator images(image_pair(m1,m2));
      odometry_object.sequence_odometry(images);
      
      waitKey(1);
    }
  inputData.unlock();
}

void chatterCallback0(const sensor_msgs::CompressedImage& msg)
{
  my_left->push_back(msg);
 callOdometry();

}

void chatterCallback1(const sensor_msgs::CompressedImage& msg)
{
  my_right->push_back(msg);
  callOdometry();
}

int main(int argc, char **argv)
{  
  string KITTI_HOME  = std::getenv("HOME");
  KITTI_HOME +=  "/catkin_ws/src/libviso/dataset";
  fs::path seq_base = fs::path(KITTI_HOME) / "sequences";
  result_dir = fs::path(KITTI_HOME) / "results" / "res_test";
  fs::create_directories(result_dir);
  string seq_name = "00";
  string calib_file_name = (seq_base/seq_name/"calib.txt").string();

  odometry_object.initiatePoses();
  
  Mat m1 = odometry_object.getP1();
  Mat m2 = odometry_object.getP2();
  bool res = loadCalib(calib_file_name, m1, m2);
  
  odometry_object.setP1(m1);
  odometry_object.setP2(m2);
  
  odometry_object.setF();
  assert(res);
/*
  odometry_object.setResultDir(result_dir);
  
  ros::init(argc, argv, "listener");
  ros::NodeHandle n;
  
  vector<sensor_msgs::CompressedImage> _left;
  vector<sensor_msgs::CompressedImage> _right;
  
  my_left  = &_left;
  my_right = &_right;
  
  ros::Subscriber sub1 = n.subscribe("/SENSORS/CAM/R/compressed", 1000, chatterCallback0);
  ros::Subscriber sub2 = n.subscribe("/SENSORS/CAM/L/compressed", 1000, chatterCallback1);
  ros::AsyncSpinner spinner(1);
   
  spinner.start();  
  ros::waitForShutdown();

  fs::path poses_dir(result_dir/"data");
  create_directories(poses_dir);
  string poses_file_name((poses_dir/(seq_name+".txt")).string());
  savePoses(poses_file_name, odometry_object.getPoses());
  
  */
  return 0;
}
